
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       pi.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "pi.h"
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void pi_ctrl_init(PI_ctrl * controler, float kp, float ki, float kimax)
{
    controler->_kp = kp;
    controler->_ki = ki;
    controler->_kimax = kimax;

    controler->output_p = 0;
    controler->integrator = 0;
}

// update controller
float pi_ctrl_update(PI_ctrl * controler, float target, float measurement, float dt)
{
    const float err = target - measurement;
    controler->integrator += controler->_ki * err * dt;
    
    controler->integrator = math_constrain_float(controler->integrator, 0, controler->_kimax);

    controler->output_p = controler->_kp * err;

    return controler->output_p + controler->integrator;
}
/*------------------------------------test------------------------------------*/


